#include <cmath>

#include <QTextStream>
#include <QVector3D>

#include "DlgGyroCalib.h"

DlgGyroCalib::DlgGyroCalib(QWidget *parent)
: QDialog(parent)
, _dev(NULL)
{
	setupUi(this);

	xSRA->setScale(0.1f);
	ySRA->setScale(0.1f);
	zSRA->setScale(0.1f);

	foreach(QObject *pobj, children())
	{
		foreach(QObject *obj, pobj->children())
		{
			QSlider *slider = qobject_cast<QSlider *>(obj);
			if(slider)
			{
				connect(slider, SIGNAL(valueChanged(int)), this, SLOT(sliderValueChanged(int)));
			}
		}
	}
}

static QString fmt(double f)
{
	QString ret;
	QTextStream ts(&ret, QIODevice::WriteOnly);
	ts.setNumberFlags(QTextStream::ForceSign);
	ts.setFieldWidth(5);
	ts.setRealNumberPrecision(3);
	ts.setRealNumberNotation(QTextStream::FixedNotation);
	ts << f;
	return ret;
}


void DlgGyroCalib::setDevice(z800_device *dev)
{
	_dev = dev;
	z800_get_calibration_data(_dev, &_data);
	for(int idx = 0; idx < 3; ++idx)
	{
		QString component = QString("xyz").mid(idx, 1);
		findChild<QLabel *>(component + "OTL")->setText(fmt(_data.gyr_offset[idx]));
		findChild<QLabel *>(component + "GTL")->setText(fmt(_data.gyr_gain[idx]));
		findChild<QSlider *>(component + "OSL")->blockSignals(true);
		findChild<QSlider *>(component + "OSL")->setValue((_data.gyr_offset[idx] + 1.0f) * 5000.f);
		findChild<QSlider *>(component + "OSL")->blockSignals(false);
		findChild<QSlider *>(component + "GSL")->blockSignals(true);
		findChild<QSlider *>(component + "GSL")->setValue(_data.gyr_gain[idx] * 250.f);
		findChild<QSlider *>(component + "GSL")->blockSignals(false);
	}
}

void DlgGyroCalib::gyroSpeedChanged(const QVector3D &gyr)
{
	xSRA->setValue(-gyr.x(), false);
	ySRA->setValue(-gyr.y(), false);
	zSRA->setValue(-gyr.z(), false);
	xSTL->setText(fmt(gyr.x()));
	ySTL->setText(fmt(-gyr.y()));
	zSTL->setText(fmt(-gyr.z()));
}

static const double Pi = 3.14159265358979323846264338327950288419717;
static double TwoPi = 2.0 * Pi;

static QString fmtDeg(double f)
{
	QString ret;
	QTextStream ts(&ret, QIODevice::WriteOnly);
	ts.setNumberFlags(QTextStream::ForceSign);
	ts.setFieldWidth(5);
	ts.setRealNumberPrecision(1);
	ts.setRealNumberNotation(QTextStream::FixedNotation);
	ts << f;
	return ret;
}

void DlgGyroCalib::eulerChanged(const QVector3D &euler)
{
	QVector3D v = euler;
	v.setX(v.x() + Pi);
	v.setY(v.y() + Pi);
	v.setZ(v.z() + Pi);
	if(v.x() < 0) v.setX(v.x() + TwoPi);
	if(v.y() < 0) v.setY(v.y() + TwoPi);
	if(v.z() < 0) v.setZ(v.z() + TwoPi);
	if(v.x() > TwoPi) v.setX(v.x() - TwoPi);
	if(v.y() > TwoPi) v.setY(v.y() - TwoPi);
	if(v.z() > TwoPi) v.setZ(v.z() - TwoPi);

	xRTL->setText(fmtDeg(euler.x() / TwoPi * 360.0));
	yRTL->setText(fmtDeg(-euler.y() / TwoPi * 360.0));
	zRTL->setText(fmtDeg(-euler.z() / TwoPi * 360.0));

	xDL->setValue(v.x() / TwoPi * 36000);
	yDL->setValue(v.y() / TwoPi * 36000);
	zDL->setValue(v.z() / TwoPi * 36000);
}

void DlgGyroCalib::sliderValueChanged(int value)
{	
	int idx;
	QString name = sender()->objectName();
	if(name[0] == 'x') idx = 0;
	else if(name[0] == 'y') idx = 1;
	else {
		Q_ASSERT(name[0] == 'z');
		idx = 2;
	}
	if(name[1] == 'O')
	{
		_data.gyr_offset[idx] = value / 5000.f - 1.0f;
		findChild<QLabel *>(name.left(2) + "TL")->setText(fmt(_data.gyr_offset[idx]));
	}
	if(name[1] == 'G')
	{
		_data.gyr_gain[idx] = value / 250.f;
		findChild<QLabel *>(name.left(2) + "TL")->setText(fmt(_data.gyr_gain[idx]));
	}
	z800_set_calibration_data(_dev, &_data);

}

